
#include "Camera.h"
#include <osgViewer/Viewer>

using namespace IG;
int Camera::id = 1;;
Camera::Camera(const CameraProps& props)
{
	myId = id++;
	//transform = new osg::MatrixTransform;
	view = new osgViewer::View;
	osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits;
	int multisample = props.multisample;
	//bool OffScreen = props.OffScreen;
	if (multisample == 0) multisample = 1;

	traits->screenNum = 0;
	traits->x = props.windowOriginX;
	traits->y = props.windowOriginY;
	int w = props.width *multisample;
	int h = props.height *multisample;
	traits->width =  w;
	traits->height =  h;
	traits->windowDecoration = props.borders;
	traits->doubleBuffer = true;
	traits->sharedContext = 0;
	traits->pbuffer = props.OffScreen;

	osg::ref_ptr<osg::GraphicsContext> gc = osg::GraphicsContext::createGraphicsContext(traits.get());
	if (gc.valid())
	{
		osg::notify(osg::INFO) << "  GraphicsWindow has been created successfully." << std::endl;
	}
	else
	{
		osg::notify(osg::NOTICE) << "  GraphicsWindow has not been created successfully." << std::endl;
	}
	osg::ref_ptr<osg::Camera> camera = view->getCamera();
	camera->setGraphicsContext(gc.get());
	camera->setViewport(new osg::Viewport(0, 0, w, h));
	//camera->setCullingActive(true);
	if (props.sensor == CAMERA_SENSOR::GRAY)
		camera->setCullMask(GRAY_SENSOR_MASK);
	else
		if (props.sensor == CAMERA_SENSOR::RGB)
			camera->setCullMask(CCD_SENSOR_MASK);
		else
			camera->setCullMask(IR_SENSOR_MASK);

	GLenum buffer = traits->doubleBuffer ? GL_BACK : GL_FRONT;
	camera->setDrawBuffer(buffer);
	camera->setReadBuffer(buffer);

	camera->setProjectionMatrixAsPerspective(props.fovY, props.fovX / props.fovY, props.nearPlane, props.farPlane);
	cb = new PreDrawCallback(NULL, (int)props.sensor);
	camera->setPreDrawCallback(cb);
	cpImage = new CaptureImageCallback( this );
	//cpImage->Print();
	camera->setPostDrawCallback(cpImage);
	//cpImage->Print();
	//if (props.multisample > 1)
	cpImage->SetDesiredSize(props.width, props.height,props.sensor == CAMERA_SENSOR::IR);


	

}


Camera::~Camera()
{
}
void Camera::SetLookAt( osg::Vec3d eye,  osg::Vec3d center,  osg::Vec3d up)
{
	osg::Matrix lookAt;
	lookAt.makeLookAt( eye , center, up );

	view->getCamera()->setViewMatrix(lookAt);
	osg::Vec3 c_eye, c_center, c_up;
	lookAt.getLookAt(c_eye, c_center, c_up);
	int x = 3;
}
void Camera::SetOffset(osg::Vec3d pos, osg::Vec3d orientation)
{
	
	osg::Matrix roll = osg::Matrix::rotate(osg::DegreesToRadians(orientation.z()), 0.0, 1.0, 0.0);
	osg::Matrix pitch = osg::Matrix::rotate(osg::DegreesToRadians(orientation.y()), 1.0, 0.0, 0.0);
	osg::Matrix heading = osg::Matrix::rotate(osg::DegreesToRadians(orientation.x()), 0.0, 0.0, 1.0);
	osg::Matrix t = osg::Matrix::translate(pos);
	//offset = roll * pitch * heading * t;
	center = pos;
//	or = orientation;
	selfRotate = roll * pitch * heading;

}
void Camera::UpdatePosition(osg::Matrixd m, osg::Matrixd rotate)
{
	osg::Vec3 nc = center *m;

	//osg::Matrixd roll = osg::Matrix::rotate(osg::DegreesToRadians(or.z() ), 0.0, 1.0, 0.0);
	//osg::Matrixd pitch = osg::Matrix::rotate(osg::DegreesToRadians(or.y()), 1.0, 0.0, 0.0);
	//osg::Matrixd heading = osg::Matrix::rotate(osg::DegreesToRadians(or.x()), 0.0, 0.0, 1.0);

	osg::Vec3 nt = osg::Vec3d(0, 1, 0) * selfRotate   *m;//check later with selfrotate pitch * heading
	
	osg::Vec3d up = osg::Vec3d(0, 0, 1)*selfRotate*rotate ;

	SetLookAt(nc, nt, up);



	
}
void Camera::SetScene(Scene* val)
{
	scene = val;
	cb->_scene = scene;

}
cv::Mat Camera::GetImage()
{

	return cpImage->GetImage();
}